/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.MecanumDriveWithJoystick;
import edu.wpi.first.wpilibj.templates.commands.TankDriveWithJoystick;

/**
 *
 * @author Eddie
 */
public class DriveTrain extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    private Jaguar 
            LEFT_FRONT_MOTOR = new Jaguar(RobotMap.DRIVE_FRONT_LEFT),
            LEFT_REAR_MOTOR = new Jaguar(RobotMap.DRIVE_REAR_LEFT),
            RIGHT_FRONT_MOTOR = new Jaguar(RobotMap.DRIVE_FRONT_RIGHT),
            RIGHT_REAR_MOTOR = new Jaguar(RobotMap.DRIVE_REAR_RIGHT);
    
    private RobotDrive drive;

    public Encoder enc_FrontLeft;
    public Encoder enc_FrontRight;
    public Encoder enc_RearLeft;
    public Encoder enc_RearRight;

    public DigitalInput EitherOne;

    AxisCamera camera;
    CriteriaCollection cc;

    double tan23 = 0.424474816;
    
    public DriveTrain() {

        super("DriveTrain");

        drive = new RobotDrive(LEFT_FRONT_MOTOR, LEFT_REAR_MOTOR, RIGHT_FRONT_MOTOR, RIGHT_REAR_MOTOR);

        drive.setSafetyEnabled(false);

        if(RobotMap.DRIVE_FRONT_LEFT_REVERSED) {
            drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        }

        if(RobotMap.DRIVE_FRONT_RIGHT_REVERSED) {
            drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        }

        if(RobotMap.DRIVE_REAR_LEFT_REVERSED) {
            drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
        }

        if(RobotMap.DRIVE_REAR_RIGHT_REVERSED) {
            drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        }

        camera = AxisCamera.getInstance("10.32.60.11");

        cc = new CriteriaCollection();
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 40, 400, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 55, 400, false);
        
        enc_FrontLeft = new Encoder(RobotMap.ENC_FRONT_LEFTA, RobotMap.ENC_FRONT_LEFTB);
        enc_FrontRight = new Encoder(RobotMap.ENC_FRONT_RIGHTA, RobotMap.ENC_FRONT_RIGHTB);
        enc_RearLeft = new Encoder(RobotMap.ENC_REAR_LEFTA, RobotMap.ENC_REAR_LEFTB);
        enc_RearRight = new Encoder(RobotMap.ENC_REAR_RIGHTA, RobotMap.ENC_REAR_RIGHTB);

        enc_FrontLeft.start();
        enc_FrontRight.start();
        enc_RearLeft.start();
        enc_RearRight.start();

        EitherOne = new DigitalInput(12);
    }

    public void atSpeed(double speed) {
        //drive.drive(speed, 0.0);
        drive.tankDrive(speed, speed);
    }

    public void strafeAtSpeed(double speed, double direction) {
        drive.mecanumDrive_Polar(0.4, direction, 0.0);
    }

    public void toTank() {
        setDefaultCommand(new TankDriveWithJoystick());
    }

    public void toMecanum() {
        setDefaultCommand(new MecanumDriveWithJoystick());
    }
    
    protected void initDefaultCommand() {
        setDefaultCommand(new MecanumDriveWithJoystick());
        EitherOne.get();
    }

    public void mecanumDriveWithJoystick() {
        drive.mecanumDrive_Polar(OI.getInstance().getDriveJoystick().getMagnitude(), OI.getInstance().getDriveJoystick().getDirectionDegrees(), OI.getInstance().getDriveJoystick2().getX());
    }
    
    public void tankDriveWithJoystick() {
        double left = -OI.getInstance().getShooterJoystick().getY();
        double right = OI.getInstance().getDriveJoystick().getY();

        drive.tankDrive(left, right);
    }

    public void changeMaxDriveSpeed(double change) {
        drive.setMaxOutput(change);
    }

    public void toDashboardEncoders() {
        SmartDashboard.putInt("Front Left", enc_FrontLeft.getRaw());
        SmartDashboard.putInt("Front Right", enc_FrontRight.getRaw());
        SmartDashboard.putInt("Rear Left", enc_RearLeft.getRaw());
        SmartDashboard.putInt("Rear Right", enc_RearRight.getRaw());
    }

    public void proportional(double desired, double current) {

        double offset = current;
        double error = desired - offset;
        double Kp = 0.02;
        SmartDashboard.putDouble("Kp", Kp);
        double[] array = {0, current};
        double output = 0.0;

        SmartDashboard.putDouble("Error", error);

        while(Math.abs(desired - offset) > 1) {
            Kp = SmartDashboard.getDouble("Kp", 0.01);

            array = findCameraDistance();
            offset = array[1];

            error = desired - current;

            output = (Kp * error);

            SmartDashboard.putDouble("Error", error);

            if(output >= 0.5)
            {
                output = 0.5;
            }

            if(output <= -0.5)
            {
                output = -0.5;
            }

            drive.tankDrive(output, -output);
        }
    } 

    public double[] findCameraDistance() {
        double distance = 0.0;
        double offset = 0.0;
        double[] array = {0, 0};
        
        try {
            ColorImage image = null;
            
            try {
                image = camera.getImage();
            } catch (AxisCameraException ex) {
            } catch (NIVisionException ex) {
            }

            BinaryImage thresholdImage = image.thresholdHSL(0, 255, 0, 31, 229, 255);   // keep only red objects
            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results

            if(reports[1] != null) {
                    ParticleAnalysisReport r = reports[1];
                    System.out.println("x: " + r.center_mass_x_normalized + " y: "  + r.center_mass_y_normalized);
                    System.out.println(r.boundingRectHeight + " " + r.boundingRectWidth);

                    distance = (9/tan23) * 240/r.boundingRectHeight;
                    System.out.println("Distance: " + distance);
                    SmartDashboard.putDouble("Distance", distance);
                    array[0] = distance;

                    offset = (46 * (160 - r.center_mass_x)) / 320;
                    System.out.println("Offset: " + offset);
                    SmartDashboard.putDouble("Offset", offset);
                    array[1] = offset;
            }

            System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());

            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();

        } catch (NIVisionException ex) {
        }

        return array;
    }
    
    public void updateStatus() {
        SmartDashboard.putDouble("Drive - Front Left", LEFT_FRONT_MOTOR.getSpeed());
        SmartDashboard.putDouble("Drive - Rear Left", LEFT_REAR_MOTOR.getSpeed());
        SmartDashboard.putDouble("Drive - Front Right", RIGHT_FRONT_MOTOR.getSpeed());
        SmartDashboard.putDouble("Drive - Rear Right", RIGHT_REAR_MOTOR.getSpeed());
        //SmartDashboard.putDouble("Camera Tilt Angle", getCameraTiltAngle());
        //SmartDashboard.putDouble("Camera Pan Angle", getCameraPanAngle());
    }
}
